/* ******************************************** */
/* * Robot Utilities Library                  * */
/* * Authors: André Bannwart Perina           * */
/* *          Antonio Canabrava Fraideinberze * */
/* *          Willian Felipe Lopes da Silva   * */
/* ******************************************** */

#include <libplayerc/playerc.h>
#include <math.h>

#include "../include/rutils.h"

/* ** Move Robot to Relative Position - Non-blocking ************************************************ */
/* * Description: Commands the robot to move to a relative position, without blocking execution     * */
/* * Inputs: client - Player / Stage Client                                                         * */
/* *         device - Position Device                                                               * */
/* *         px - Units to move on x axis                                                           * */
/* *         py - Units to move on y axis                                                           * */
/* *         pa - Units to move angularly                                                           * */
/* *         vx - x Axis Speed                                                                      * */
/* *         vy - y Axis Speed                                                                      * */
/* *         va - Angular Speed                                                                     * */
/* * Output: Return value from playerc_position2d_set_cmd_pose_with_vel                             * */
/* ************************************************************************************************** */
inline int rutils_move_rel_nblk(playerc_client_t *client, playerc_position2d_t *device, double px, double py, double pa, double vx, double vy, double va) {
	player_pose2d_t pos, vel;

	/* Calculate relative position */
	pos.px = (px*cos((device->pa)-(M_PI/2))-py*sin((device->pa)-(M_PI/2)))+(device->px);
	pos.py = (px*sin((device->pa)-(M_PI/2))+py*cos((device->pa)-(M_PI/2)))+(device->py);
	pos.pa = pa+(device->pa);

	/* Define pose structure */
	vel.px = vx;
	vel.py = vy;
	vel.pa = va;

	/* Move */
	return playerc_position2d_set_cmd_pose_with_vel(device, pos, vel, 1);
}

/* ** Move Robot to Relative Position - Blocking **************************************************** */
/* * Description: Commands the robot to move to a relative position, blocking execution until a     * */
/* *              tolerable position is reached                                                     * */
/* * Inputs: client - Player / Stage Client                                                         * */
/* *         device - Position Device                                                               * */
/* *         px - Units to move on x axis                                                           * */
/* *         py - Units to move on y axis                                                           * */
/* *         pa - Units to move angularly                                                           * */
/* *         vx - x Axis Speed                                                                      * */
/* *         vy - y Axis Speed                                                                      * */
/* *         va - Angular Speed                                                                     * */
/* *         pxTol - Tolerable distance on x axis to unblock execution                              * */
/* *         pyTol - Tolerable distance on y axis to unblock execution                              * */
/* *         paTol - Tolerable angular distance to unblock execution                                * */
/* * Output: Return value from playerc_position2d_set_cmd_pose_with_vel                             * */
/* ************************************************************************************************** */
int rutils_move_rel_blk(playerc_client_t *client, playerc_position2d_t *device, double px, double py, double pa, double vx, double vy, double va, double pxTol, double pyTol, double paTol) {
	int retVal;
	double rpx, rpy, rpa;

	/* Wait for the robot to stop moving (or to be at least moving less than the tolerance) */
	do {
		rpx = device->px;
		rpy = device->py;
		rpa = device->pa;
		playerc_client_read(client);
	} while(fabs(device->px - rpx) > pxTol || fabs(device->py - rpy) > pyTol || fabs(device->pa - rpa) > paTol);

	/* Move */
	retVal = rutils_move_rel_nblk(client, device, px, py, pa, vx, vy, va);

	/* Wait for the robot to stop moving (or to be at least moving less than the tolerance) */
	do {
		rpx = device->px;
		rpy = device->py;
		rpa = device->pa;
		playerc_client_read(client);
	} while(fabs(device->px - rpx) > pxTol || fabs(device->py - rpy) > pyTol || fabs(device->pa - rpa) > paTol);

	return retVal;
}

/* ** Rotate Robot to Relative Heading - Non-blocking *********************************************** */
/* * Description: Commands the robot to rotate to a relative heading, without blocking execution    * */
/* * Inputs: client - Player / Stage Client                                                         * */
/* *         device - Position Device                                                               * */
/* *         pa - Units to move angularly                                                           * */
/* *         va - Angular Speed                                                                     * */
/* * Output: Return value from playerc_position2d_set_cmd_pose_with_vel                             * */
/* ************************************************************************************************** */
inline int rutils_rotate_rel_nblk(playerc_client_t *client, playerc_position2d_t *device, double pa, double va) {
	player_pose2d_t pos, vel;

	/* Calculate relative heading */
	pos.px = device->px;
	pos.py = device->py;
	pos.pa = pa+(device->pa);

	/* Define pose structure */
	vel.px = 0;
	vel.py = 0;
	vel.pa = va;

	/* Move */
	return playerc_position2d_set_cmd_pose_with_vel(device, pos, vel, 1);
}

/* ** Rotate Robot to Relative Heading - Blocking *************************************************** */
/* * Description: Commands the robot to rotate to a relative heading, blocking execution until a    * */
/* *              tolerable heading is reached                                                      * */
/* * Inputs: client - Player / Stage Client                                                         * */
/* *         device - Position Device                                                               * */
/* *         pa - Units to move angularly                                                           * */
/* *         va - Angular Speed                                                                     * */
/* *         paTol - Tolerable angular distance to unblock execution                                * */
/* * Output: Return value from playerc_position2d_set_cmd_pose_with_vel                             * */
/* ************************************************************************************************** */
int rutils_rotate_rel_blk(playerc_client_t *client, playerc_position2d_t *device, double pa, double va, double paTol) {
	int retVal;
	double rpa;

	/* Wait for the robot to stop moving (or to be at least moving less than the tolerance) */
	do {
		rpa = device->pa;
		playerc_client_read(client);
	} while(fabs(device->pa - rpa) > paTol);

	/* Move */
	retVal = rutils_rotate_rel_nblk(client, device, pa, va);

	/* Wait for the robot to stop moving (or to be at least moving less than the tolerance) */
	do {
		rpa = device->pa;
		playerc_client_read(client);
	} while(fabs(device->pa - rpa) > paTol);

	return retVal;
}